#include "i2c.h"
#include "uart.h"
#include "itg3200.h"
#include "status.h"

/* IGT3200 I2C address */
i2c_device_t itg3200 =
{ ITG3200_I2C_ADDRESS };

/**
 * Initialize gyroscope sensor
 *
 * @param void
 * @return void
 *
 * @brief  get device id of the gyroscope sensor
 */
void itg3200_init(void)
{
	uint8_t data;

	/* Low pass filter bandwidth of 42Hz */
	data = LPFBW_5HZ | LPBFW_STD;
	itg3200_write_reg(DLPF_FS_REG, &data);

	/* Internal sample rate of 200Hz. (1kHz / 5) */
	data = SAMPLE_RATE_200;
	itg3200_write_reg(SMPLRT_DIV_REG, &data);
}

/**
 * Read Gyroscope Device ID
 *
 * @param uint8_t *id Device ID
 * @return uint32_t i2c read status
 *
 * @brief  get device id of the gyroscope sensor
 */
uint32_t itg3200_read_id(uint8_t *id)
{
	return itg3200_read_reg(WHO_AM_I_REG, id);
}

/**
 * Get Gyroscope output data
 *
 * @param int16_t *readings data for xyz readings
 * @return uint32_t i2c read status
 *
 * @brief  get x y z values
 */
uint32_t itg3200_read_xyz(int16_t *readings)
{
	uint32_t ret;
	uint8_t value[6];
	/* read all values in one i2c access */
	ret = i2c_read_burst(&itg3200, GYRO_XOUT_H_REG, value);
	/* convert values */
	readings[0] = ((value[0] << 8) | value[1]);
	readings[1] = ((value[2] << 8) | value[3]);
	readings[2] = ((value[4] << 8) | value[5]);
	return ret;
}

/**
 * Gyroscope Register Read
 *
 * @param uint8_t reg device register address
 * @param uint8_t *data
 * @return uint32_t i2c read status
 *
 * @brief  read gyro register value
 */
uint32_t itg3200_read_reg(uint8_t reg, uint8_t *data)
{
	return i2c_read(&itg3200, reg, data);
}

/**
 * Gyroscope Register Write
 *
 * @param uint8_t reg device register address
 * @param uint8_t *data
 * @return i2c write status
 *
 * @brief  get gyro register value
 */
uint32_t itg3200_write_reg(uint8_t reg, uint8_t *data)
{
	return i2c_write(&itg3200, reg, data);
}

